#include "stdafx.h"
// Bloomenthal.cpp: Bloomenthal 
//
//////////////////////////////////////////////////////////////////////

#include "Bloomenthal.h"

using namespace polygonizer;

//static INTLISTS *cubetable[256];
static struct Bloomenthal::intlists *cubetable[256];

/*			edge: LB, LT, LN, LF, RB, RT, RN, RF, BN, BF, TN, TF */
static int corner1[12]	   = {LBN,LTN,LBN,LBF,RBN,RTN,RBN,RBF,LBN,LBF,LTN,LTF};
static int corner2[12]	   = {LBF,LTF,LTN,LTF,RBF,RTF,RTN,RTF,RBN,RBF,RTN,RTF};
static int leftface[12]	   = {B,  L,  L,  F,  R,  T,  N,  R,  N,  B,  T,  F};
/* face on left when going corner1 to corner2 */
static int rightface[12]   = {L,  T,  N,  L,  B,  R,  R,  F,  B,  F,  N,  T};
/* face on right when going corner1 to corner2 */

Bloomenthal::Bloomenthal()
{

}

Bloomenthal::~Bloomenthal()
{

}


double Bloomenthal::torus(double x, double y, double z)
{
	double x2 = x*x, y2 = y*y, z2 = z*z;
	double a = x2+y2+z2+(0.5*0.5)-(0.1*0.1);
	return a*a-4.0*(0.5*0.5)*(y2+z2);
}

double Bloomenthal::sphere(double x, double y, double z)
{
	double rsq = x*x+y*y+z*z;
	return 1.0/(rsq < 0.00001? 0.00001 : rsq);
}


double Bloomenthal::blob(double x, double y, double z)
{
	return 4.0-sphere(x+1.0,y,z)-sphere(x,y+1.0,z)-sphere(x,y,z+1.0);
}


int Bloomenthal::triproc(int i1, int i2, int i3, VERTICES vertices)
{
	gvertices = vertices;
	gntris++;

	//  if(gntris%100 == 0)
	//    printf("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b  %d tiriangles", gntris); 
	fprintf(tri_file, "%d %d %d\n", i1, i2, i3);

	return 1;
}	

/**** An Implicit Surface Polygonizer ****/


/* polygonize: polygonize the implicit surface function
*   arguments are:
*	 double function (x, y, z)
*		 double x, y, z (an arbitrary 3D point)
*	     the implicit surface function
*	     return negative for inside, positive for outside
*	 double size
*	     width of the partitioning cube
*	 int bounds
*	     max. range of cubes (+/- on the three axes) from first cube
*	 double x, y, z
*	     coordinates of a starting point on or near the surface
*	     may be defaulted to 0., 0., 0.
*	 int triproc (i1, i2, i3, vertices)
*		 int i1, i2, i3 (indices into the vertex array)
*		 VERTICES vertices (the vertex array, indexed from 0)
*	     called for each triangle
*	     the triangle coordinates are (for i = i1, i2, i3):
*		 vertices.ptr[i].position.x, .y, and .z
*	     vertices are ccw when viewed from the out (positive) side
*		 in a left-handed coordinate system
*	     vertex normals point outwards
*	     return 1 to continue, 0 to abort
*	 int mode
*	     TET: decompose cube and polygonize six tetrahedra
*	     NOTET: polygonize cube directly
*   returns error or NULL
*/

char* Bloomenthal::polygonize(PolyMesh* mesh, double size, float bounds[6], double x, double y, double z, int mode)
{
	gntris = 0;

	ver_file = fopen(VERFILE, "w");
	tri_file = fopen(TRIFILE, "w");


	PROCESS p;
	int n, noabort;
	//CORNER *setcorner();
	TEST1  *in, *out;// find();

	//p.function = this->function;//function;
	//p.triproc = triproc;
	p.size = size;
	//p.bounds = bounds;
	p.boundsX1 = bounds[0];
	p.boundsX2 = bounds[1];
	p.boundsY1 = bounds[2];
	p.boundsY2 = bounds[3];
	p.boundsZ1 = bounds[4];
	p.boundsZ2 = bounds[5];
	p.delta = size/(double)(RES*RES);

	/* allocate hash tables and build cube polygon table: */
	p.centers = (CENTERLIST **) mycalloc(HASHSIZE,sizeof(CENTERLIST *));
	p.corners = (CORNERLIST **) mycalloc(HASHSIZE,sizeof(CORNERLIST *));
	p.edges =	(EDGELIST   **) mycalloc(2*HASHSIZE,sizeof(EDGELIST *));
	makecubetable();

	/* find point on surface, beginning search at (x, y, z): */
	srand(1);
	in = find1(1, &p, x, y, z);
	out = find1(0, &p, x, y, z);
	if (!in->ok || !out->ok) 
		return "can't find starting point";
	converge(&in->p, &out->p, in->value, &p.start);

	delete in;
	delete out;

	/* push initial cube on stack: */
	p.cubes = (CUBES *) mycalloc(1, sizeof(CUBES)); /* list of 1 */
	p.cubes->cube.i = p.cubes->cube.j = p.cubes->cube.k = 0;
	p.cubes->next = NULL;

	/* set corners of initial cube: */
	for (n = 0; n < 8; n++)
		p.cubes->cube.corners[n] = setcorner1(&p, BIT(n,2), BIT(n,1), BIT(n,0));

	p.vertices.count = p.vertices.max = 0; /* no vertices yet */
	p.vertices.ptr = NULL;

	setcenter(p.centers, 0, 0, 0);

	while (p.cubes != NULL) { /* process active cubes till none left */
		CUBE c;
		CUBES *temp = p.cubes;
		c = p.cubes->cube;

		noabort = mode == TET?
			/* either decompose into tetrahedra and polygonize: */
			dotet(&c, LBN, LTN, RBN, LBF, &p) &&
			dotet(&c, RTN, LTN, LBF, RBN, &p) &&
			dotet(&c, RTN, LTN, LTF, LBF, &p) &&
			dotet(&c, RTN, RBN, LBF, RBF, &p) &&
			dotet(&c, RTN, LBF, LTF, RBF, &p) &&
			dotet(&c, RTN, LTF, RTF, RBF, &p)
			:
		/* or polygonize the cube directly: */
		docube(&c, &p);
		if (! noabort) return "aborted";

		/* pop current cube from stack */
		p.cubes = p.cubes->next;
		free((char *) temp);

		/* test six face directions, maybe add to stack: */
		testface(c.i-1, c.j, c.k, &c, L, LBN, LBF, LTN, LTF, &p);
		testface(c.i+1, c.j, c.k, &c, R, RBN, RBF, RTN, RTF, &p);
		testface(c.i, c.j-1, c.k, &c, B, LBN, LBF, RBN, RBF, &p);
		testface(c.i, c.j+1, c.k, &c, T, LTN, LTF, RTN, RTF, &p);
		testface(c.i, c.j, c.k-1, &c, N, LBN, LTN, RBN, RTN, &p);
		testface(c.i, c.j, c.k+1, &c, F, LBF, LTF, RBF, RTF, &p);

		for(n=0; n<8; n++)
			free((char *)c.corners[n]);
	}

	//free
	int i;
	for(i=0; i<HASHSIZE; i++){
		while(p.centers[i] != NULL){
			CENTERLIST* current = p.centers[i];
			p.centers[i] = current->next;
			free((char*)current);
		}
	}
	free((char*)p.centers);

	for(i=0; i<HASHSIZE; i++){
		while(p.corners[i] != NULL){
			CORNERLIST* current = p.corners[i];
			p.corners[i] = current->next;
			free((char*)current);
		}
	}
	free((char*)p.corners);

	for(i=0; i<2*HASHSIZE; i++){
		while(p.edges[i] != NULL){
			EDGELIST* current = p.edges[i];
			p.edges[i] = current->next;
			free((char*)current);
		}
	}
	free((char*)p.edges);

	//out
	int vertex_N = gvertices.count;
	int face_N = gntris;
	fclose(ver_file);
	fclose(tri_file);
	ver_file = fopen(VERFILE, "r");
	tri_file = fopen(TRIFILE, "r");

	mesh->setSize(vertex_N, face_N);
	for(i=0; i<vertex_N; i++){
		float vx, vy, vz;
		fscanf(ver_file, "%f %f %f", &vx, &vy, &vz);
		mesh->setVertex(i, vx, vy, vz);
	}
	for(i=0; i<face_N; i++){
		int i0, i1, i2;
		fscanf(tri_file, "%d %d %d", &i0, &i1, &i2);
		mesh->setFace(i, i0, i1, i2);
	}
	fclose(ver_file);
	fclose(tri_file);

	//  printf("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b %d tiriangles\n", gntris);

	return NULL;
}

/* testface: given cube at lattice (i, j, k), and four corners of face,
* if surface crosses face, compute other four corners of adjacent cube
* and add new cube to cube stack */

void Bloomenthal::testface(int i, int j, int k, CUBE *old, 
	int face, int c1, int c2, int c3, int c4, PROCESS *p)
{
	CUBE new1;
	CUBES *oldcubes = p->cubes;
	//CORNER *setcorner();
	static int facebit[6] = {2, 2, 1, 1, 0, 0};
	int n, pos = old->corners[c1]->value > 0.0 ? 1 : 0, bit = facebit[face];

	/* test if no surface crossing, cube out of bounds, or already visited: */
	if ((old->corners[c2]->value > 0) == pos &&
		(old->corners[c3]->value > 0) == pos &&
		(old->corners[c4]->value > 0) == pos) return;
	//if (abs(i) > p->bounds || abs(j) > p->bounds || abs(k) > p->bounds) return;
	float size = (float)p->size;
	if(i*size + p->start.x > p->boundsX1 
		|| i*size + p->start.x < p->boundsX2 
		|| j*size + p->start.y > p->boundsY1 
		|| j*size + p->start.y < p->boundsY2 
		|| k*size + p->start.z > p->boundsZ1 
		|| k*size + p->start.z < p->boundsZ2)
		return;
	if (setcenter(p->centers, i, j, k)) return;

	/* create new cube: */
	new1.i = i;
	new1.j = j;
	new1.k = k;
	/*
	for (n = 0; n < 8; n++) new1.corners[n] = NULL;
	new1.corners[FLIP(c1, bit)] = old->corners[c1];
	new1.corners[FLIP(c2, bit)] = old->corners[c2];
	new1.corners[FLIP(c3, bit)] = old->corners[c3];
	new1.corners[FLIP(c4, bit)] = old->corners[c4];*/
	for (n = 0; n < 8; n++)
		//if (new1.corners[n] == NULL)
		new1.corners[n] = setcorner1(p, i+BIT(n,2), j+BIT(n,1), k+BIT(n,0));

	/*add cube to top of stack: */
	p->cubes = (CUBES *) mycalloc(1, sizeof(CUBES));
	p->cubes->cube = new1;
	p->cubes->next = oldcubes;
}

/* setcorner: return corner with the given lattice location
set (and cache) its function value */
Bloomenthal::CORNER * Bloomenthal::setcorner1(PROCESS *p, int i, int j, int k)
{
	/* for speed, do corner value caching here */
	CORNER *c = (CORNER *) mycalloc(1, sizeof(CORNER));
	int index = HASH(i, j, k);
	CORNERLIST *l = p->corners[index];
	c->i = i; c->x = p->start.x+((double)i-.5)*p->size;
	c->j = j; c->y = p->start.y+((double)j-.5)*p->size;
	c->k = k; c->z = p->start.z+((double)k-.5)*p->size;
	for (; l != NULL; l = l->next)
		if (l->i == i && l->j == j && l->k == k) {
			c->value = l->value;
			return c;
		}
		l = (CORNERLIST *) mycalloc(1, sizeof(CORNERLIST));
		l->i = i; l->j = j; l->k = k;
		l->value = c->value = function(c->x, c->y, c->z);
		l->next = p->corners[index];
		p->corners[index] = l;
		return c;
}

/* find: search for point with value of given sign (0: neg, 1: pos) */
Bloomenthal::TEST1* Bloomenthal::find1(int sign1, PROCESS *p, double x, double y, double z)
{
	int i;
	TEST1* test = new TEST1;
	double range = p->size;
	test->ok = 1;
	for (i = 0; i < 100000; i++) {
		test->p.x = x+range*(RAND()-0.5);
		test->p.y = y+range*(RAND()-0.5);
		test->p.z = z+range*(RAND()-0.5);
		test->value = function(test->p.x, test->p.y, test->p.z);
		if (sign1 == (test->value > 0.0)) return test;
		range = range*1.0005; /* slowly expand search outwards */
	}
	test->ok = 0;
	return test;
}

/**** Tetrahedral Polygonization ****/

/* dotet: triangulate the tetrahedron
* b, c, d should appear clockwise when viewed from a
* return 0 if client aborts, 1 otherwise */

int Bloomenthal::dotet(CUBE *cube, int c1, int c2, int c3, int c4, PROCESS *p)
{
	CORNER *a = cube->corners[c1];
	CORNER *b = cube->corners[c2];
	CORNER *c = cube->corners[c3];
	CORNER *d = cube->corners[c4];
	int index = 0, apos, bpos, cpos, dpos, e1, e2, e3, e4, e5, e6;
	if (apos = (a->value > 0.0)) index += 8;
	if (bpos = (b->value > 0.0)) index += 4;
	if (cpos = (c->value > 0.0)) index += 2;
	if (dpos = (d->value > 0.0)) index += 1;
	/* index is now 4-bit number representing one of the 16 possible cases */
	if (apos != bpos) e1 = vertid(a, b, p);
	if (apos != cpos) e2 = vertid(a, c, p);
	if (apos != dpos) e3 = vertid(a, d, p);
	if (bpos != cpos) e4 = vertid(b, c, p);
	if (bpos != dpos) e5 = vertid(b, d, p);
	if (cpos != dpos) e6 = vertid(c, d, p);
	/* 14 productive tetrahedral cases (0000 and 1111 do not yield polygons */
	switch (index) {
	case 1:	 return triproc(e5, e6, e3, p->vertices);
	case 2:	 return triproc(e2, e6, e4, p->vertices);
	case 3:	 return triproc(e3, e5, e4, p->vertices) &&
				 triproc(e3, e4, e2, p->vertices);
	case 4:	 return triproc(e1, e4, e5, p->vertices);
	case 5:	 return triproc(e3, e1, e4, p->vertices) &&
				 triproc(e3, e4, e6, p->vertices);
	case 6:	 return triproc(e1, e2, e6, p->vertices) &&
				 triproc(e1, e6, e5, p->vertices);
	case 7:	 return triproc(e1, e2, e3, p->vertices);
	case 8:	 return triproc(e1, e3, e2, p->vertices);
	case 9:	 return triproc(e1, e5, e6, p->vertices) &&
				 triproc(e1, e6, e2, p->vertices);
	case 10: return triproc(e1, e3, e6, p->vertices) &&
				 triproc(e1, e6, e4, p->vertices);
	case 11: return triproc(e1, e5, e4, p->vertices);
	case 12: return triproc(e3, e2, e4, p->vertices) &&
				 triproc(e3, e4, e5, p->vertices);
	case 13: return triproc(e6, e2, e4, p->vertices);
	case 14: return triproc(e5, e3, e6, p->vertices);
	}
	return 1;
}

/**** Cubical Polygonization (optional) ****/

/* docube: triangulate the cube directly, without decomposition */

int Bloomenthal::docube(CUBE *cube, PROCESS *p)
{
	INTLISTS *polys;
	int i, index = 0;
	for (i = 0; i < 8; i++) if (cube->corners[i]->value > 0.0) index += (1<<i);
	for (polys = cubetable[index]; polys; polys = polys->next) {
		INTLIST *edges;
		int a = -1, b = -1, count = 0;
		for (edges = polys->list; edges; edges = edges->next) {
			CORNER *c1 = cube->corners[corner1[edges->i]];
			CORNER *c2 = cube->corners[corner2[edges->i]];
			int c = vertid(c1, c2, p);
			if (++count > 2 && ! triproc(a, b, c, p->vertices)) return 0;
			if (count < 3) a = b;
			b = c;
		}
	}
	return 1;
}

/* nextcwedge: return next clockwise edge from given edge around given face */

int Bloomenthal::nextcwedge(int edge, int face)
{
	switch (edge) {
	case LB: return (face == L)? LF : BN;
	case LT: return (face == L)? LN : TF;
	case LN: return (face == L)? LB : TN;
	case LF: return (face == L)? LT : BF;
	case RB: return (face == R)? RN : BF;
	case RT: return (face == R)? RF : TN;
	case RN: return (face == R)? RT : BN;
	case RF: return (face == R)? RB : TF;
	case BN: return (face == B)? RB : LN;
	case BF: return (face == B)? LB : RF;
	case TN: return (face == T)? LT : RN;
	case TF: return (face == T)? RT : LF;
	default: return 0;
	}
}

/* otherface: return face adjoining edge that is not the given face */

int Bloomenthal::otherface(int edge, int face)
{
	int other = leftface[edge];
	return face == other? rightface[edge] : other;
}

/* makecubetable: create the 256 entry table for cubical polygonization */

void Bloomenthal::makecubetable()
{
	static int visited = 0;

	if (visited)
		return;

	visited = 1;

	int i, e, c, done[12], pos[8];
	for (i = 0; i < 256; i++) {
		for (e = 0; e < 12; e++) done[e] = 0;
		for (c = 0; c < 8; c++) pos[c] = BIT(i, c);
		for (e = 0; e < 12; e++)
			if (!done[e] && (pos[corner1[e]] != pos[corner2[e]])) {
				INTLIST *ints = 0;
				INTLISTS *lists = (INTLISTS *) mycalloc(1, sizeof(INTLISTS));
				int start = e, edge = e;
				/* get face that is to right of edge from pos to neg corner: */
				int face = pos[corner1[e]]? rightface[e] : leftface[e];
				while (1) {
					edge = nextcwedge(edge, face);
					done[edge] = 1;
					if (pos[corner1[edge]] != pos[corner2[edge]]) {
						INTLIST *tmp = ints;
						ints = (INTLIST *) mycalloc(1, sizeof(INTLIST));
						ints->i = edge;
						ints->next = tmp; /* add edge to head of list */
						if (edge == start) break;
						face = otherface(edge, face);
					}
				}
				lists->list = ints; /* add ints to head of table entry */
				lists->next = cubetable[i];
				cubetable[i] = lists;
			}
	}
}

/**** Storage ****/

/* mycalloc: return successful calloc or exit program */

char * Bloomenthal::mycalloc(int nitems, int nbytes)
{
	char *ptr = (char*)calloc(nitems, nbytes);
	if (ptr != NULL) return ptr;
	fprintf(stderr, "can't calloc %d bytes\n", nitems*nbytes);
	exit(1);
}

/* setcenter: set (i,j,k) entry of table[]
* return 1 if already set; otherwise, set and return 0 */

int Bloomenthal::setcenter(CENTERLIST *table[], int i, int j, int k)
{
	int index = HASH(i, j, k);
	CENTERLIST *new1, *l, *q = table[index];
	for (l = q; l != NULL; l = l->next)
		if (l->i == i && l->j == j && l->k == k) return 1;
	new1 = (CENTERLIST *) mycalloc(1, sizeof(CENTERLIST));
	new1->i = i; new1->j = j; new1->k = k; new1->next = q;
	table[index] = new1;
	return 0;
}

/* setedge: set vertex id for edge */

void Bloomenthal::setedge(EDGELIST *table[], int i1, int j1, int k1, int i2, int j2, int k2, int vid)
{
	unsigned int index;
	EDGELIST *new1;
	if (i1>i2 || (i1==i2 && (j1>j2 || (j1==j2 && k1>k2)))) {
		int t=i1; i1=i2; i2=t; t=j1; j1=j2; j2=t; t=k1; k1=k2; k2=t;
	}
	index = HASH(i1, j1, k1) + HASH(i2, j2, k2);
	new1 = (EDGELIST *) mycalloc(1, sizeof(EDGELIST));
	new1->i1 = i1; new1->j1 = j1; new1->k1 = k1;
	new1->i2 = i2; new1->j2 = j2; new1->k2 = k2;
	new1->vid = vid;
	new1->next = table[index];
	table[index] = new1;	
}

/* getedge: return vertex id for edge; return -1 if not set */

int Bloomenthal::getedge(EDGELIST *table[], int i1, int j1, int k1, int i2, int j2, int k2)
{
	EDGELIST *q;
	if (i1>i2 || (i1==i2 && (j1>j2 || (j1==j2 && k1>k2)))) {
		int t=i1; i1=i2; i2=t; t=j1; j1=j2; j2=t; t=k1; k1=k2; k2=t;
	};
	q = table[HASH(i1, j1, k1)+HASH(i2, j2, k2)];
	for (; q != NULL; q = q->next)
		if (q->i1 == i1 && q->j1 == j1 && q->k1 == k1 &&
			q->i2 == i2 && q->j2 == j2 && q->k2 == k2)
			return q->vid;
	return -1;
}

/**** Vertices ****/


/* vertid: return index for vertex on edge:
* c1->value and c2->value are presumed of different sign
* return saved index if any; else compute vertex and save */

int Bloomenthal::vertid(CORNER *c1, CORNER *c2, PROCESS *p)
{
	VERTEX v;
	POINT1 a, b;
	int vid = getedge(p->edges, c1->i, c1->j, c1->k, c2->i, c2->j, c2->k);
	if (vid != -1) return vid;			     /* previously computed */
	a.x = c1->x; a.y = c1->y; a.z = c1->z;
	b.x = c2->x; b.y = c2->y; b.z = c2->z;

	double x1, y1, z1, x2, y2, z2, f1, f2;
	if(c1->value > 0){
		f1 = c1->value;
		x1 = c1->x; y1 = c1->y; z1 = c1->z;
		f2 = c2->value;
		x2 = c2->x; y2 = c2->y; z2 = c2->z;
	}
	else{
		f1 = c2->value;
		x1 = c2->x; y1 = c2->y; z1 = c2->z;
		f2 = c1->value;
		x2 = c1->x; y2 = c1->y; z2 = c1->z;
	}

	//Repeated Linear Interpolation
	double x, y, z, f;
	for(int i=0; i<10; i++){
		double w1 = fabs(f1);
		double w2 = fabs(f2);
		x = (w2*x1 + w1*x2)/(w1+w2);
		y = (w2*y1 + w1*y2)/(w1+w2);
		z = (w2*z1 + w1*z2)/(w1+w2);

		if(accuracy == 0)
			break;

		f = function(x,y,z);
		if(fabs(f) < accuracy)
			break;
		if(f > 0){
			f1 = f;
			x1 = x;  y1 = y;  z1 = z;
		}
		else{
			f2 = f;
			x2 = x;  y2 = y;  z2 = z;
		}
	}
	v.position.x = x;
	v.position.y = y;
	v.position.z = z;



	//converge(&a, &b, c1->value, &v.position); /* position */
	//vnormal(&v.position, p, &v.normal);			   /* normal */
	addtovertices(&p->vertices, v);			   /* save vertex */
	vid = p->vertices.count-1;
	setedge(p->edges, c1->i, c1->j, c1->k, c2->i, c2->j, c2->k, vid);
	return vid;
}

/* addtovertices: add v to sequence of vertices */

void Bloomenthal::addtovertices(VERTICES *vertices, VERTEX v)
{
	fprintf(ver_file, "%f %f %f\n", v.position.x, v.position.y, v.position.z);
	vertices->count++;
	/*
	if(vertices->count == vertices->max) {
	int i;
	VERTEX *new1;
	vertices->max = vertices->count == 0 ? 10 : 2*vertices->count;
	new1 = (VERTEX *) mycalloc((unsigned) vertices->max, sizeof(VERTEX));
	for (i = 0; i < vertices->count; i++) new1[i] = vertices->ptr[i];
	if (vertices->ptr != NULL) free((char *)vertices->ptr);
	vertices->ptr = new1;
	}
	vertices->ptr[vertices->count++] = v;
	}*/
}

/* vnormal: compute unit length surface normal at point */

void Bloomenthal::vnormal(POINT1 *point, PROCESS *p, POINT1 *v)
{
	double f = function(point->x, point->y, point->z);
	v->x = function(point->x+p->delta, point->y, point->z)-f;
	v->y = function(point->x, point->y+p->delta, point->z)-f;
	v->z = function(point->x, point->y, point->z+p->delta)-f;
	f = sqrt(v->x*v->x + v->y*v->y + v->z*v->z);
	if (f != 0.0) {v->x /= f; v->y /= f; v->z /= f;}
}

/* converge: from two points of differing sign, converge to zero crossing */
void Bloomenthal::converge(POINT1 *p1, POINT1 *p2, double v, POINT1 *p)
{

	int i = 0;
	POINT1 pos, neg;
	if (v < 0) {
		pos.x = p2->x; pos.y = p2->y; pos.z = p2->z;
		neg.x = p1->x; neg.y = p1->y; neg.z = p1->z;
	}
	else {
		pos.x = p1->x; pos.y = p1->y; pos.z = p1->z;
		neg.x = p2->x; neg.y = p2->y; neg.z = p2->z;
	}
	while (1) {
		p->x = 0.5*(pos.x + neg.x);
		p->y = 0.5*(pos.y + neg.y);
		p->z = 0.5*(pos.z + neg.z);
		if (i++ == RES) return;
		if ((function(p->x, p->y, p->z)) > 0.0)
		{pos.x = p->x; pos.y = p->y; pos.z = p->z;}
		else {neg.x = p->x; neg.y = p->y; neg.z = p->z;}
	}
}

double Bloomenthal::function(double x, double y, double z)
{
	return func->value((float)x, (float)y, (float)z);  
}
